#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>

#include "ControlledObject.h"
#include "Sprite.h"
#include "GL/glut.h"
#include "include.h"

#define abs(num) num<0?num*-1.0:num
#define TEXTURE_FILE "./Images/lincoln.bmp"
#define POSE_FILE "./Images/lincolnpose.bmp"
void ControlledObject::SetHealth(float health)
{
	this->health = health;
}
float ControlledObject::GetHealth()
{
	return health;
}
void ControlledObject::AddForceObjectSpace(Vector2D ray, Vector2D origin, bool groundForce)
{
	if(ray.Magnitude() == 0.0)
		return;
	
	float forceStrength = ray.Magnitude()/forceDamageDenominator;
	
	//printf("Health = %f\n", health);
	//printf("force = %f\n", forceStrength);
	if(forceStrength > 1 && (origin.dist(vertices[2]) > 0.1 && origin.dist(vertices[3]) > 0.1))
	{
		health -= forceStrength;
	}
	Vector2D towardCenter = -1.0 * origin;
	Vector2D unitTowardCenter = !towardCenter; 
	Vector2D unitRay = !ray;

	float cosTheta = ray.dot(unitTowardCenter);//unitTowardCenter.dot(ray);
	
	Vector2D rayTowardCenter = (cosTheta) * unitTowardCenter;
	//rotation = 0.0;

	if(GetFreeRotate())
	{
		AddForce(rayTowardCenter);
		Vector2D rayRotational = (ray - rayTowardCenter);
		torque += rayRotational.x * towardCenter.y - rayRotational.y * towardCenter.x;
	}
	else
	{
		AddForce(ray);
	}
	
	//If ray is "mostly up" we should add friction and move the bottom vertices so
	//we're touching the ground.
	printf("Checking ground force\n");
	if(groundForce)
	{
		float angle = atan2(ray.y, ray.x);

		//the incline angle he is capable of running up should be dependent on his
		//velocity in his forward direction.
		Vector2D horizontal = !(vertices[1] - vertices[0]);
		float horizontalVelocity = horizontal.dot(velocity);
		if(horizontalVelocity < 0)
			horizontalVelocity *= -1;
		//30 degrees = 0.523598333 radians
		printf("horizontalVelocity = %f\n", horizontalVelocity);
		float smallAngle = 45 - horizontalVelocity/100.0 * 25;
		if(smallAngle < 20)
			smallAngle = 20;
		float largeAngle = 180 - smallAngle;
		smallAngle *= PI/180;
		largeAngle *= PI/180;
		if(angle >= smallAngle && angle <= largeAngle) //between 45 and 135 degrees
		{
			hitGround = true;
			isOnGround = true;
			isOnRightWall = false;
			isOnLeftWall = false;
			if(!GetFreeRotate())
			{
				Vector2D objectUp = vertices[1] - vertices[2];
				float bottomAngle = atan2(objectUp.y, objectUp.x);//acos(orthogonal.dot(bottom));
				bottomAngle = angle - bottomAngle;
				
				rotation += bottomAngle;
				rotateCount++;
			}
		}
		else if(!movingRight && !isOnGround && (angle >= -0.5853975 && angle < 0.5853975))// && !isOnRightWall && !isOnGround)
		{
			isOnLeftWall = true;
			Vector2D objectSide = vertices[1] - vertices[0];
			float sideAngle = atan2(objectSide.y, objectSide.x);
			sideAngle = angle - sideAngle;
			
			//rotation += sideAngle;
			//rotateCount++;
		}
		else if(!movingLeft && !isOnGround && (angle <= -2.5561925 || angle > 2.5561925))// && !isOnGround && !isOnLeftWall)
		{
			isOnRightWall = true;
			if(angle < 0)
				angle += 2 * 3.14159;
			Vector2D objectSide = vertices[0] - vertices[1];
			float sideAngle = atan2(objectSide.y, objectSide.x);
			sideAngle = angle - sideAngle;
			
			//rotation += sideAngle;
			//rotateCount++;
		}
		else
		{
			isOnHead = true;
		}
		
		if(isOnRightWall || isOnLeftWall)
		{
			Vector2D wallFriction = !velocity;
			wallFriction = -WALL_FRICTION_FORCE * wallFriction * mass;
			Vector2D right = vertices[1] - vertices[0];
			right = !right * WALL_FRICTION_FORCE * mass;
			if(isOnRightWall)
				wallFriction = wallFriction + right;
			else
				wallFriction = wallFriction - right;
			AddForce(wallFriction);
		}
	}
}
void ControlledObject::AddForceObjectSpace(Vector2D ray, Vector2D origin)
{
	AddForceObjectSpace(ray, origin, true);
}
ControlledObject::~ControlledObject()
{
	printf("deleting controlled object\n");
	if(imageVertices)
		delete(imageVertices);
	imageVertices = NULL;
	printf("deleting controlled object2\n");
	if(animationManager)
		delete(animationManager);
	animationManager = NULL;
	printf("deleting controlled object3\n");
	if(material)
		delete(material);
	material = NULL;
	printf("done deleting controlled object\n");
}
ControlledObject::ControlledObject( TextureManager * textureManager,
			Vector2D position, 
			Vector2D velocity,
			Vector2D acceleration,
			float rotation,
			float angularVelocity,
			float angularAcceleration,
			float mass,
			bool isPinned)
{
	forceDamageDenominator = 1000000.0;
	health = 100;
	maxHealth = 100;
	this->textureManager = textureManager;
	lastRotation = 0.0;
	isNotOnGroundCount = 0;
	movingTimeModifier = 8.0;
	isOnGroundCount = 0;
	isOnLeftWallCount = 0;
	isOnRightWallCount = 0;
	facingRight = true;
	movingLeft = false;
	movingRight = false;
	objectId = CONTROLLEDOBJECT_CLASS;
	spriteList = new SpriteList();
	material = new Material("ControlledObjectTexture",0,0,true,0.1,-1);
	rotateCount = 0;
	
	this->position = position;
	this->velocity = velocity;
	this->acceleration = acceleration;
	vertices = (Vector2D *)malloc(4*sizeof(Vector2D));
	assert(vertices != NULL);
	
	vertices[0] = Vector2D(-2.0, 7.0);
	vertices[1] = Vector2D(2.0, 7.0);
	vertices[2] = Vector2D(2.0, -7.0);
	vertices[3] = Vector2D(-2.0, -7.0);
	imageVertices = vertices;
	this->numVertices = 4;
	this->rotation = rotation;
	this->angularVelocity = angularVelocity;
	this->angularAcceleration = angularAcceleration;
	this->mass = mass;
	this->isPinned = isPinned;	
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	CalculateMomentOfInertia();
}
/* ControlledObject::CalculateUpVector()
{
	//sideNormals point into the object
	sideNormals[0] = Vector2D(0,-1);
	sideNormals[1] = Vector2D(-1,0);	
	sideNormals[2] = Vector2D(0,1);
	sideNormals[3] = Vector2D(1,0);
}*/
bool ControlledObject::GetFreeRotate()
{
	return false;
}

void ControlledObject::Rotate(float theta)//, int side)
{
	//return;
	//0 = top
	//1 = right
	//2 = bottom
	//3 = left
	if(isPinned)
		return;
		
	if(1)//GetFreeRotate())
	{
		while(theta < 0)
			theta += 2.0 * 3.14159;
			
		if(abs(theta) < 0.000001)
			return;
		float sinTheta = sin(theta);
		float cosTheta = cos(theta);
		
		for(int i = 0; i < numVertices; i++)
		{
			float newX = vertices[i].x * cosTheta - vertices[i].y * sinTheta;
			float newY = vertices[i].y * cosTheta + vertices[i].x * sinTheta;
			vertices[i].x = newX;
			vertices[i].y = newY;
		}	
		
		if(vertices != imageVertices)
		{
			for(int i = 0; i < 4; i++)
			{
				float newX = imageVertices[i].x * cosTheta - imageVertices[i].y * sinTheta;
				float newY = imageVertices[i].y * cosTheta + imageVertices[i].x * sinTheta;
				imageVertices[i].x = newX;
				imageVertices[i].y = newY;
			}
		}
		
		float x = upVector.x * cosTheta - upVector.y * sinTheta;
		float y = upVector.y * cosTheta + upVector.x * sinTheta;
		upVector.x = x;
		upVector.y = y;
		
		if(keyPointList)
		{
			keyPointList->Rotate(sinTheta, cosTheta);
		}
		if(collisionValid)
		{
			float x = collisionPoint.x * cosTheta - collisionPoint.y * sinTheta;
			float y = collisionPoint.y * cosTheta + collisionPoint.x * sinTheta;
			collisionPoint.x = x;
			collisionPoint.y = y;
		}
		
		ControlledObjectRotateEntryPoint(cosTheta, sinTheta);
	}
	/*else
	{
		while(theta < 0)
			theta += 2.0 * 3.14159;
		
		int side = 2;
		int index = side;
		int index2 = (side+1)%numVertices;
		if(abs(theta) < 0.000001)
			return;
			
		float sinTheta = sin(theta);
		float cosTheta = cos(theta);
		
		Vector2D bottomMiddle = (vertices[index] + vertices[index2]) / 2.0;
		for(int i = 0; i <= numVertices; i++)
		{
			int c = i;//(side + i)%numVertices;
			float newX = vertices[c].x * cosTheta - vertices[c].y * sinTheta;
			float newY = vertices[c].y * cosTheta + vertices[c].x * sinTheta;
			vertices[c].x = newX;
			vertices[c].y = newY;
			//vertices[c] = vertices[c] + bottomMiddle;
		}	
		for(int i = 0; i < 4; i++)
		{
			float newX = imageVertices[i].x * cosTheta - imageVertices[i].y * sinTheta;
			float newY = imageVertices[i].y * cosTheta + imageVertices[i].x * sinTheta;
			imageVertices[i].x = newX;
			imageVertices[i].y = newY;
		}
		
	}*/
}
float ControlledObject::GetAnimationSpeed()
{
	if(isOnGround && velocity.Magnitude() > 0.0)
		return movingTimeModifier/velocity.Magnitude();
	return -1;
}
Vector2D * ControlledObject::GetWallPoseCoordinates()
{
	Vector2D * texCoord = (Vector2D *)malloc(4 * sizeof(Vector2D));
	texCoord[0] = Vector2D(0.25,1);
	texCoord[1] = Vector2D(0, 1);
	texCoord[2] = Vector2D(0, 0);
	texCoord[3] = Vector2D(0.25, 0);
	return texCoord;
}
void ControlledObject::Draw()
{	
	ControlledObjectDrawEntryPoint();
	glPushMatrix();
	glLoadIdentity();
	glColor3f(1,1,1);
	glTranslatef(position.x,position.y, 0.0);
	if(textureManager)
	{
		if(!isOnGround && (isOnRightWall || isOnLeftWall))
			textureManager->EnableTexture(poseIndex);
		else
			textureManager->EnableTexture(textureIndex);
	}
		
	glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
	glAlphaFunc( GL_GREATER, 0.5 );
  	glEnable( GL_ALPHA_TEST );


	Vector2D * texCoord = NULL;
	
	if(!isOnGround && isOnRightWall)
	{
		texCoord = GetWallPoseCoordinates();
		
	}
	else if(!isOnGround && isOnLeftWall)
	{
		texCoord = GetWallPoseCoordinates();
		Vector2D temp = texCoord[0];
		texCoord[0] = texCoord[1];
		texCoord[1] = temp;
		temp = texCoord[2];
		texCoord[2] = texCoord[3];
		texCoord[3] = temp;
	}
	else
	{
		if(animationManager)
			texCoord = animationManager->GetVertices();
		else
		{
			texCoord = (Vector2D *)malloc(sizeof(Vector2D)*4);
			memcpy(texCoord,textureVertices, sizeof(Vector2D)*4);
		}
		
		if((GetFreeRotate()||(facingRight && velocity.x < -10) || (!facingRight && velocity.x < 10)))
		{
			facingRight = false;
			Vector2D temp = texCoord[0];
			texCoord[0] = texCoord[1];
			texCoord[1] = temp;
			temp = texCoord[2];
			texCoord[2] = texCoord[3];
			texCoord[3] = temp;
		}
		else
			facingRight = true;
	}
	
	glEnable(GL_TEXTURE_2D);
	glBegin(GL_QUADS);
		
	glTexCoord2f(texCoord[0].x,texCoord[0].y);
	glVertex2f(imageVertices[0].x, imageVertices[0].y);
	
	glTexCoord2f(texCoord[1].x,texCoord[1].y);
	glVertex2f(imageVertices[1].x, imageVertices[1].y);
	
	glTexCoord2f(texCoord[2].x,texCoord[2].y);
	glVertex2f(imageVertices[2].x, imageVertices[2].y);
	
	glTexCoord2f(texCoord[3].x,texCoord[3].y);
	glVertex2f(imageVertices[3].x, imageVertices[3].y);
	glEnd();
	
	glPopMatrix();
	glDisable(GL_TEXTURE_2D);
	if(texCoord)
	{
		delete(texCoord);
		texCoord = NULL;
	}
	
	spriteList->Draw();
	
}
void ControlledObject::DrawCollision()
{
	if(collisionValid)
	{
		
		Vector2D velAtPoint = collisionPoint + VelocityAtPoint(collisionPoint);
		//return;
		glDisable(GL_TEXTURE_2D);
		glDisable( GL_ALPHA_TEST );
		glPushMatrix();
		glLoadIdentity();
		glTranslatef(position.x,position.y, 0.0);
		glColor3f(0,1,0);
		glBegin(GL_LINES);
		Vector2D(0,0);
		Vector2D(10,10);
		Vector2D(collisionPoint.x, collisionPoint.y);
		Vector2D(velAtPoint.x, velAtPoint.y);
		glEnd();
		glPopMatrix();
	}
}
void ControlledObject::MoveRight()
{
	Vector2D right = vertices[1] - vertices[0];
	right = !right;
	float rightSpeed = right.dot(velocity);
	if(rightSpeed > 500)
		return;
	movingRight = true;
	isOnLeftWall = false;
	if(isOnGround)
	{
		float multiplier = velocity.Magnitude() * MOVE_MULTIPLIER;
		if(multiplier > MAX_MOVE_FORCE)
			multiplier = MAX_MOVE_FORCE;
		else if(multiplier < abs(GRAVITATIONAL_FORCE) * mass)
			multiplier = abs(GRAVITATIONAL_FORCE) * mass;
		AddForce(right * multiplier * mass);
	}
	else if(isOnRightWall)
	{
		//AddForce(right * 500 * mass);
	}
	else
	{
		if(rightSpeed>100)
		{
			movingRight = false;
			return;
		}
		AddForce(right * AIR_MOVE_FORCE * mass);
	} 
}
void ControlledObject::MoveLeft()
{
	isOnRightWall = false;
	Vector2D left = vertices[0] - vertices[1];
	left = !left;
	float leftSpeed = left.dot(velocity);
	if(leftSpeed > 500)
		return;
	movingLeft = true;
	if(isOnGround)
	{
		float multiplier = velocity.Magnitude() * MOVE_MULTIPLIER;
		if(multiplier > MAX_MOVE_FORCE)
			multiplier = MAX_MOVE_FORCE;
		else if(multiplier < abs(GRAVITATIONAL_FORCE) * mass)
			multiplier = abs(GRAVITATIONAL_FORCE) * mass;
		AddForce(left * multiplier * mass);
	}
	else if(isOnLeftWall)
	{
		//AddForce(left * 500 * mass);
	}
	else
	{
		if(leftSpeed > 100)
		{
			movingLeft = false;
			return;
		}
		AddForce(left * AIR_MOVE_FORCE * mass);
	}
}
void ControlledObject::Jump()
{
	printf("Jumping\n");
	if(isOnGround)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D up = Vector2D(0,1);//vertices[1] - vertices[2];
		up = !up * JUMP_FORCE * mass;
	
		AddForce(up);
	}
	else if(isOnRightWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D left = vertices[0] - vertices[1];
		left = (!left + Vector2D(0,1)) * 0.5;
		left = left * WALLJUMP_FORCE * mass;
		AddForce(left);
	}
	else if(isOnLeftWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D right = vertices[1] - vertices[0];
		right = (!right + Vector2D(0,1)) * 0.5;
		right = right * WALLJUMP_FORCE * mass;
		AddForce(right);
	}
	else
	{
	}
	isOnGround = false;
}
void ControlledObject::AddGravity()
{
	Vector2D gravitationalForce = Vector2D(0.0, GRAVITATIONAL_FORCE) * mass;
	
	AddForce(gravitationalForce);
	ControlledObjectAddGravityEntryPoint();
}
bool ControlledObject::PreThinkEntryPoint(double elapsedTime)
{
	ControlledObjectThinkEntryPoint(elapsedTime);
	return false;
}
void ControlledObject::AddDamage(float damage)
{
	health-= damage;
}
void ControlledObject::ThinkEntryPoint(double elapsedTime)
{
	if(animationManager)
	{
		animationManager->SetTimeBetweenFrames(GetAnimationSpeed());
		//printf("controlled torque = %f\n", torque);
		//printf("RotateCount = %d\n", rotateCount);
		animationManager->AddElapsedTime(elapsedTime);
	}
	if(!isOnGround && !isOnRightWall && !isOnLeftWall)
	{
		isOnGroundCount = 0;
		isOnLeftWallCount = 0;
		isOnRightWallCount = 0;
		isNotOnGroundCount++;
		if(!GetFreeRotate() && isNotOnGroundCount > 10)
		{
			Vector2D objectUp = vertices[1] - vertices[2];
			float bottomAngle = atan2(objectUp.y, objectUp.x);
			bottomAngle = 3.14159 * 0.5 - bottomAngle;
				 
			//printf("bottom angle = %f\n", bottomAngle);
			if(bottomAngle > 0.1047)
				bottomAngle = 0.1047;
			else if(bottomAngle < -0.1047)
				bottomAngle = -0.1047;
				
			if(abs(bottomAngle) > 0.087267)
				Rotate(bottomAngle);
		}
		
	}
	else
	{
		bool canRotate = false;
		if(isOnLeftWall)
		{
			isOnLeftWallCount++;
			isNotOnGroundCount++;
			isOnRightWallCount = 0;
			isOnGroundCount = 0;
			if(isOnLeftWallCount > 5)
				canRotate = true;
			spriteList->Add(new Sprite(position + vertices[3], Vector2D(3,3), Vector2D(1,1), 0.05, textureManager, DUST_FILE));
		}
		else if(isOnRightWall)
		{
			isOnRightWallCount++;
			isNotOnGroundCount++;
			isOnGroundCount = 0;
			isOnLeftWallCount = 0;
			if(isOnRightWallCount > 5)
				canRotate = true;
			spriteList->Add(new Sprite(position + vertices[2], Vector2D(3,3), Vector2D(1,1), 0.05, textureManager, DUST_FILE));
		}
		else if(isOnGround)
		{
			isOnGroundCount ++;
			isNotOnGroundCount = 0;
			isOnLeftWallCount = 0;
			isOnRightWallCount = 0;
			if(isOnGroundCount > 5)
				canRotate = true;
		}
		
		if(rotateCount > 0 && !GetFreeRotate())
		{
			rotation = rotation/(float)rotateCount;

			lastRotation = rotation;
			if(rotation > 0.1047)
				rotation = 0.1047;
			else if(rotation < -0.1047)
				rotation = -0.1047;
			if(abs(rotation) > 0.087267)
			{
				Rotate(rotation);
			}
		}
	}
	
	if(GetFreeRotate())
	{
		rotation = angularVelocity * elapsedTime;
		while(rotation > 2 * 3.14159)
			rotation -= 2 * 3.14159;
		Rotate(rotation);
	}
	//ControlledObjectThinkEntryPoint(elapsedTime);
	spriteList->Cleanup();
	rotateCount = 0;
	rotation = 0.0;
	movingRight = false;
	movingLeft = false;
	isOnHead = false;
}
Vector2D ControlledObject::VelocityAtPoint(Vector2D point)
{
	return velocity;
}
void ControlledObject::ControlledObjectThinkEntryPoint(double elapsedTime)
{
}
void ControlledObject::ControlledObjectDrawEntryPoint()
{
}
void ControlledObject::ControlledObjectRotateEntryPoint(float cosTheta, float sinTheta)
{
}
void ControlledObject::ControlledObjectAddGravityEntryPoint()
{
}
	